#! /usr/bin/env python3

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__=="__main__":
    rospy.init_node("frames_sub_p")
    buffer=tf2_ros.Buffer()
    listence=tf2_ros.TransformListener(buffer)

    rate=rospy.Rate(1)
    
    while not rospy.is_shutdown():
        try:
            # 1. 获取 son1 和 son2 之间的相对关系(这里设置son1是父级坐标系,son2是子级坐标系)
            tfs=buffer.lookup_transform("son1","son2",rospy.Time(0))
            rospy.loginfo("son1与son2的相对关系为: ")
            rospy.loginfo("父级坐标为: %s",tfs.header.frame_id)
            rospy.loginfo("子级坐标为: %s",tfs.child_frame_id)
            rospy.loginfo("时间戳: %s",tfs.header.stamp)
            rospy.loginfo("相对关系: %s",tfs.transform)
            rospy.loginfo("相对坐标关系:x=%0.2f,y=%0.2f,z=%0.2f",tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z)
            # 2. 创建一依赖于 son1 的坐标点
            point_source = PointStamped()#创建一个坐标点
            point_source.header.frame_id = "son1"#点依赖于son1坐标系
            point_source.header.stamp = rospy.Time.now()
            point_source.point.x = 1
            point_source.point.y = 1
            point_source.point.z = 1
            #将上述创建的的位置关系从son1坐标系转换到son2坐标系
            point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))

            rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)
            rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z
            )
        
        except Exception as e:
            rospy.logerr("Error: %s",e)
        rate.sleep()


